#include "diff_drive_robot/diff_drive_hardware_interface.h"

namespace diff_drive_robot
{
DiffDriveHardwareInterface::DiffDriveHardwareInterface(ros::NodeHandle& nh, urdf::Model* urdf_model)
  : ros_control_boilerplate::GenericHWInterface(nh, urdf_model)
{
  ROS_INFO("DiffDriveHardwareInterface constructed");
}

void DiffDriveHardwareInterface::init()
{
  // 调用父类的 init 函数
  ros_control_boilerplate::GenericHWInterface::init();

  // 初始化硬件接口
  ROS_INFO("DiffDriveHardwareInterface initialized");

  // 示例: 为关节设置初始位置和速度
  joint_position_ = {0.0, 0.0};
  joint_velocity_ = {0.1, 0.1};
  joint_effort_ = {0.0, 0.0};
  joint_position_command_ = {0.0, 0.0};
  joint_velocity_command_ = {0.1, 0.1};
  joint_effort_command_ = {0.0, 0.0};
}

void DiffDriveHardwareInterface::read(ros::Duration& elapsed_time)
{
  // 模拟从硬件读取数据
  joint_position_[0] += joint_velocity_[0] * elapsed_time.toSec();
  joint_position_[1] += joint_velocity_[1] * elapsed_time.toSec();

  // 调试日志
  ROS_INFO_STREAM("Read joint positions: [" << joint_position_[0] << ", " << joint_position_[1] << "]");
}

void DiffDriveHardwareInterface::write(ros::Duration& elapsed_time)
{
  // 模拟将命令写入硬件
  joint_velocity_[0] = joint_velocity_command_[0];
  joint_velocity_[1] = joint_velocity_command_[1];

  // 调试日志
  ROS_INFO_STREAM("Write joint velocity commands: [" << joint_velocity_command_[0] << ", " << joint_velocity_command_[1] << "]");
}

void DiffDriveHardwareInterface::enforceLimits(ros::Duration& period)
{
  // 在这里实现硬件限制的逻辑
  // 例如，确保速度在允许的范围内
  // 这里的实现会根据你的具体硬件和控制要求而不同
}
}  // namespace diff_drive_robot
